#include "rads_rtk.hpp"

// 全局原子变量，用于线程间的通信
std::atomic<bool> should_stop(false);
std::condition_variable cv;
std::mutex mtx;

// 主要是获取网络来源的差分数据 
void threadrtk_qianxun(std_msgs::String& nema_msg,QxDiffData& diff_data,std_msgs::Int16 &rtcm) {
	TCPSocket socket;  
	socket.initialize();
	std::string genGPGGA = "";
	char check_buffer[2048];
	size_t check_length; 
	bool connecting = false;
	bool qx_auth = false;
	while(!should_stop.load())
	{
		if(!connecting)
		{
			// 千寻服务初始化
			if (socket.connectToServer(QX_SERVER, QX_PORT)) {
				connecting = true;	
				std::string auth = authCode();
				if(socket.sendData(auth.c_str(),auth.size()))
				{
					if (socket.receiveData(check_buffer, sizeof(check_buffer),check_length)) {  
						std::string checkCode = check_buffer;
						if(0 == checkCode.find(ICY_200_OK))qx_auth = true;
					}  	
				}
			} 
		}
		if(qx_auth)
		{
			// 差分数据收发服务
			std::vector<std::string> result = splitString(nema_msg.data);  
			if(GPYBM_LENGTH == result.size()){
				NEMA_GPYBM gpybm = nema2gpybm(result);
                if(std::isnan(safe_stod(gpybm.lat))||std::isnan(safe_stod(gpybm.lng)))
                {
                    #ifdef DEV_LOCAL
                        ROS_ERROR("Judging the vehicle may be inside,NEMA_GPYBM is NaN");
                    #endif
                }else{
				    genGPGGA = generateGPGGA(safe_stod(gpybm.lat),safe_stod(gpybm.lng),gpybm.fix_master);
				    socket.sendData(genGPGGA.c_str(),genGPGGA.size());
				    socket.receiveData(diff_data.data, DIFF_DATA_LEN,diff_data.len);
			        rtcm.data = diff_data.len;
				    diff_data.is_ready = 1;
                }
			}
		}
	    std::unique_lock<std::mutex> lck(mtx);
	    cv.wait_for(lck, std::chrono::milliseconds(1000), [] { return should_stop.load(); });
	}	
}

// 读取串口NEMA_GPGGA数据并且输入差分数据
void threadrtk_core(ros::NodeHandle& nh,std_msgs::String& nema_msg,QxDiffData& diff_data,rads::rads_trackext& nav ) {
	bool serial_dev_status = false;
	std::string line;  
	SerialPortRtk serial(RTK_DEVICE, B115200); 
	while (!should_stop.load()) {
		try {
			if(!serial_dev_status){
				if (serial.open()) {
                    serial_dev_status = true;
                }else{
				    ROS_WARN("[rtk] is running,but serial open [%s] fail.",std::string(RTK_DEVICE).c_str());
                }
				std::this_thread::sleep_for(std::chrono::seconds(RTK_CHECK_SEC));  
			}else{
				if (serial.readLine(line)) { 
					if(serial.startsWith(line,PREFIX_GPYBM))
					{
						std::vector<std::string> result = splitString(line);  
						if(GPYBM_LENGTH == result.size()){
							NEMA_GPYBM gpybm = nema2gpybm(result);
							nema_msg.data = line;
                            // 经纬度，解算情况，航向
							nav.latitude = safe_stod(gpybm.lat);
							nav.longitude = safe_stod(gpybm.lng);
							nav.altitude = safe_stod(gpybm.alt);
                            nav.posi_flag =  gpybm.fix_master;
                            nav.head_flag =  gpybm.fix_slave;
                            nav.heading = safe_stod(gpybm.yaw);
                            nav.yaw = safe_stod(gpybm.yaw);
                            nav.groud_speed = gpybm.spd_groud;
                            nav.matching_rate = -1;
                            // 全局北向X,全局东向
                            nav.x = gpybm.gauss_nx;
                            nav.y = gpybm.gauss_ey;
                            nav.z = safe_stod(gpybm.alt);
                            // 地向速度(ext)
                            nav.ext = gpybm.spd_groud;
                            // 基线长度(flag)
                            nav.flag = gpybm.base_length;
							if(diff_data.is_ready)
							{
#if CURRENT_DEBUG_LEVEL > DEBUG_LEVEL_INFO
							//	ROS_INFO("Sending differential data size(%zu) .",diff_data.len);
#endif
								serial.writeLine(diff_data.data,diff_data.len);
								diff_data.is_ready = 0;
							}
						}
					}
				    std::unique_lock<std::mutex> lck(mtx);
				    cv.wait_for(lck, std::chrono::milliseconds(10), [] { return should_stop.load(); });
				} else {  
					std::cerr << "Failed to read line from serial port" << std::endl;  
					break;  
				}  
			}  
		} catch (const std::exception& e) {  
			ROS_ERROR("Thread caught an exception: %s", e.what());  
			break;  
		} catch (...) {  
			ROS_ERROR("Thread caught an unknown exception");  
			break;  
		}  
	}  
}  

// 信号处理函数
void signalHandler(int signum) {
    if(SIGINT == signum)
    {
        system("killall -s SIGTERM rads_guard");
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        ROS_INFO("[Rtk] Interrupt signal (%d) received. Shutting down...", signum);
        std::lock_guard<std::mutex> lck(mtx);
        should_stop.store(true);
        cv.notify_all();
        ros::shutdown();
    }
}

int main(int argc, char **argv)
{
    ROS_INFO("[Rtk] Starting...");
	ros::init(argc, argv, NODE_RTK);
	ros::NodeHandle nh;
    // 设置信号处理函数
    signal(SIGINT, signalHandler);
	std_msgs::String nema0183_gpybm;
	QxDiffData diff_data;
	rads::rads_trackext nav;
    std_msgs::Int16 rtcm;
    // 获取GNSS定位数据
	std::thread t_rtk(threadrtk_core,std::ref(nh),std::ref(nema0183_gpybm),std::ref(diff_data),std::ref(nav));
    // 申请千寻服务数据
	std::thread t_rtk_qx(threadrtk_qianxun,std::ref(nema0183_gpybm),std::ref(diff_data),std::ref(rtcm));
	// NAV定位基本信息话题发布
    ros::Publisher p = nh.advertise<rads::rads_trackext>("/pradsrtk", QUEUE_SIZE);
    // 话题发布的rtcm字节数
    ros::Publisher p2 = nh.advertise<std_msgs::Int16>("/pradsrtcm", QUEUE_SIZE);
	ros::Rate rate(5);
	while (nh.ok() && !should_stop.load())
	{
		nav.header.stamp = ros::Time::now();
	    nav.header.frame_id = "k827";
		p.publish(nav);
        p2.publish(rtcm);
		ros::spinOnce();
		rate.sleep();
	}
    if(t_rtk.joinable())t_rtk.join();
    if(t_rtk_qx.joinable())t_rtk_qx.join();
	return 0;
}
